import numpy as np
import matplotlib.pyplot as plt
import seaborn as sns
import control as ct
from math import cos,sin,tan
def get_G_pqr(B_long,B_la):
    G_pqr=np.zeros((3,3))
    #G_pqr[[0,2],[1,2]]=B_la[[1,2],[0,1]]
    G_pqr[0,1]=B_la[1,0]
    G_pqr[0,2]=B_la[1,1]
    G_pqr[2,1]=B_la[2,0]
    G_pqr[2,2]=B_la[2,1]
    G_pqr[1,0]=B_long[2,0]
    return G_pqr
def get_G_alphabetaphi(A_long,A_la):
    G_alphabetaphi=np.zeros((3,3))
    G_alphabetaphi[0,1]=A_long[1,2]
    G_alphabetaphi[1,0]=A_la[0,1]
    G_alphabetaphi[1,2]=A_la[0,2]
    G_alphabetaphi[2,0]=A_la[3,1]
    G_alphabetaphi[2,2]=A_la[3,2]
    #print(G_alphabetaphi)
    return G_alphabetaphi
import numpy as np
import matplotlib.pyplot as plt

def plot_multiple_y(x, ys, titles, y2s=None, plot_y2s=0, figname="default", x_label=None, y_labels=None, layout=None):
    """
    绘制多个因变量的子图，并添加轴标记和子图题目（可选轴标记）。
    
    参数:
    x : x轴的数据
    ys : 一个二维数组，第一维表示因变量，第二维表示每个因变量随时间的变化
    titles : 每个子图的标题，列表形式
    x_label : x轴的通用标签（可选）
    y_labels : 每个因变量y轴的标签，列表形式（可选）
    layout : 子图的布局，例如 (行数, 列数)（可选）
    """
    num_vars = ys.shape[0]  # 获取因变量的数量
    if plot_y2s:
        if y2s is None:
            raise ValueError("y2s should not be none")
        if y2s.shape[0] != num_vars:
            raise ValueError("y2s must have the same number of rows as ys")
    if layout is None:
        rows = int(np.ceil(np.sqrt(num_vars)))
        cols = int(np.ceil(num_vars / rows))
    else:
        rows, cols = layout
        
    fig, axs = plt.subplots(rows, cols, figsize=(5 * cols, 5 * rows))
    axs = axs.flatten()  # 将axs数组扁平化，便于迭代处理

    for i in range(num_vars):
        ax = axs[i]
        ax.plot(x, ys[i, :], label="NDI")  # ys[i, :] 表示第i个变量随时间的变化
        if plot_y2s:
            ax2 = ax.twinx()  # 创建第二个y轴
            ax2.plot(x, y2s[i, :], label="open", color='orange')
            ax2.set_ylabel('y2 axis label')  # 可以是y2s的特定标签
            ax2.legend(loc='upper right')
        ax.set_title(titles[i])
        if x_label:
            ax.set_xlabel(x_label)
        if y_labels:
            ax.set_ylabel(y_labels[i])
        ax.grid(True)
        ax.legend(loc='upper left')

    # 隐藏多余的子图
    for j in range(i + 1, len(axs)):
        axs[j].axis('off')
    fig.suptitle(figname)
    plt.tight_layout()
    
    plt.savefig("figs/"+figname+".svg")
    #plt.show()
    return fig, axs

def get_ABCD(longdata,ladata,basicdata,mach):
    D_v=longdata.iloc[0]['D_V']
    #mach=0.8
    g=basicdata['g']
    m=basicdata['mass']
    Ixx=basicdata['Ixx']
    Iyy=basicdata['Iyy']
    Izz=basicdata['Izz']
    Ixz=basicdata['Ixz']
    Inertia=np.array([[Ixx,0,Ixz],[0,Iyy,0],[Ixz,0,Izz]])
    positions = longdata.index[longdata['mach'] == mach]

    longdata=longdata.iloc[positions,:]
    ladata=ladata.iloc[positions,:]

    D_a=longdata.iloc[0]['D_a']
    alpha0=longdata.iloc[0]['alpha0']
    L_a=longdata.iloc[0]['L_a']
    L_v=longdata.iloc[0]['L_V']
    T=longdata.iloc[0]['T']
    v0=longdata.iloc[0]['v0']
    M_a=longdata.iloc[0]['M_a']
    M_a_dot=longdata.iloc[0]['M_a_dot']
    M_q=longdata.iloc[0]['M_q']
    M_de=longdata.iloc[0]['M_de']
    n_a=longdata.iloc[0]['n_a']
    T=longdata.iloc[0]['T']
    
    L_b=ladata.iloc[0]['L_b']
    L_p=ladata.iloc[0]['L_p']
    L_r=ladata.iloc[0]['L_r']
    L_da=ladata.iloc[0]['L_da']
    L_dr=ladata.iloc[0]['L_dr']
    N_b=ladata.iloc[0]['N_b']
    N_p=ladata.iloc[0]['N_p']
    N_r=ladata.iloc[0]['N_r']
    N_da=ladata.iloc[0]['N_da']
    N_dr=ladata.iloc[0]['N_dr']
    Y_b=ladata.iloc[0]['Y_b']
    Y_dr=ladata.iloc[0]['Y_dr']
#long_matrix
    A_long=[[-D_v,g-D_a-T*sin(alpha0)/m,0,-g],
   [-1*L_v/v0,-1*L_a/v0-T*cos(alpha0)/m/v0,1,0],
   [-M_a_dot*L_v/v0,M_a-M_a_dot*(L_a/v0+T*cos(alpha0)/m/v0),M_a_dot+M_q,0],
   [0,0,1,0]
    ];
    B_long=[0,0,M_de,0]
    A_long=np.array(A_long)
    B_long=np.array(B_long).reshape(4,1)
    C_long=np.eye(4)
    D_long=np.zeros((A_long.shape[1],1))
#print(A_long,B_long,C_long,D_long)

#la_matrix
    omega=1-Ixz**2/Ixx/Izz
    A_la=[
    [Y_b/v0,sin(alpha0),-cos(alpha0),g*cos(alpha0)/v0],
    [(L_b+Ixz/Ixx*N_b)/omega,(L_p+Ixz/Ixx*N_p)/omega,(L_r+Ixz/Ixx*N_r)/omega,0],
    [(N_b+Ixz/Izz*L_b)/omega,(N_p+Ixz/Izz*L_p)/omega,(N_r+Ixz/Izz*L_r)/omega,0],
    [0,1,tan(alpha0),0]
    ]
    B_la=[
    [0,Y_dr/v0],
    [(L_da+Ixz/Ixx*N_da)/omega,(L_dr+Ixz/Ixx*N_dr)/omega],
    [(N_da+Ixz/Izz*L_da)/omega,(N_dr+Ixz/Izz*L_dr)/omega],
    [0,0]
    ]
    A_la=np.array(A_la)
    B_la=np.array(B_la)
    C_la=np.eye(4).T
    D_la=np.zeros((A_la.shape[1],2))
    return A_long,B_long,C_long,D_long,A_la,B_la,C_la,D_la,n_a,T,L_a,L_v,Y_b,v0,alpha0
def get_(sys,T,delta_desire_alpha,delta_desire_beta,delta_desire_phi,G_pqr,G_alphabetaphi):
    t,y = ct.input_output_response(
        sys, T,
        #U
        [delta_desire_alpha, delta_desire_beta, delta_desire_phi], 
        #X0
        np.zeros((14,)),
        #return_x=True,
        params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi})

    #print(y.shape)
#print(System.connection_table(show_names=1))
    fig1,ax1=plot_multiple_y(t,y[:8,:],titles=["v","alpha","q","theta","beta","p","r","phi"])
#print(state)
    fig,ax=plot_multiple_y(t,y[8:,:],
                       titles=["desired_p","desired_q","desired_r","de","da","dr"])
    return


if "__main__" == __name__:
    x = np.linspace(0, 10, 100)
    ys = np.random.randn(8, 100)  # 假设有8个因变量
    titles = [f'Plot of Variable {i+1}' for i in range(8)]
    x_label = 'Time (s)'
    y_labels = [f'Variable {i+1} Amplitude' for i in range(8)]  # 仅为示例，可以省略

# 使用函数
    plot_multiple_y(x, ys, titles, x_label, y_labels)